
//---------------------------------------------------------------- INCLUDE

//-------------------------------------------------------- Include système
using namespace std;
#include <stdlib.h>
#include <time.h>
#include <iostream>
#include <fstream>
#include <math.h>



//------------------------------------------------------ Include personnel
#include "Robot.h"
float test = 0;

//------------------------------------------------------------- Constantes

//----------------------------------------------------------------- PUBLIC

//----------------------------------------------------- Méthodes publiques
Reseau* Robot::GetReseau()
{
  return monCerveau;
}

float Robot::GetFitness()
{
  return fitness;
}

void Robot::SetFitness(float newFitness)
{
  fitness = newFitness;
}

void Robot::Save(char* name)
{
  float fitnessTemp;

  ofstream outfile;
  outfile.open (name, ofstream::out);

  ifstream infile;
  infile.open (name, ifstream::in);
  infile.read ( (char*) &fitnessTemp, sizeof (fitness ) );
 
  if(fitnessTemp < fitness)
  {
     outfile.write ( (char*) &fitness, sizeof ( fitness ) );
     monCerveau->Save(outfile);
  }
  
}

void Robot::CalculerFitness(structInfosSortie infosSortie[NB_JEUX])
{
  int nbButs = 0;
  int nbDepassements = 0;
  float xBalleTotal = 0;
  float yBalleTotal = 0;
  float xBalleMoyenne = 0;
  float yBalleMoyenne = 0;

  for(int i = 0 ; i < NB_JEUX ; i++)
  {
    if(infosSortie[i].butMarque)
    {
      nbButs++;
    }
    if(infosSortie[i].depassement)
    {
      nbDepassements++;
    }
    xBalleTotal += infosSortie[i].xBalle;
    yBalleTotal += infosSortie[i].yBalle;
  }
  xBalleMoyenne = xBalleTotal/NB_JEUX;
  yBalleMoyenne = yBalleTotal/NB_JEUX;


}


structInfosSortie Robot::Jouer(webots::Supervisor* pMaster,webots::Field* coordKhep1,webots::Field* coordKhep2,webots::Field* coordBall,double coordBordsTerrain [4][2],double radiusBall)
{
bool depFlag=false;
int butFlag=0;
const double *vecTemp;
double * coucheSortie;
structInfosSortie infosSortie;
double infosEntrees[12];
for(int i=0;i<12;i++)
  {
      infosEntrees[i]=0;
  }
  bool arret = false;
  int nbIterations = 0;
// Récupération des infos en entrées : position balle et position robot dans un tableau de doubles infosEntrees
 #ifdef SIM
    //Pour chaque objet sur le terrain simulés,recupere lapositiondel'ojetenquestion,puiscalcullavitesseparraport a la position précedentes puis garde la nouvelle position.

 vecTemp=coordKhep1->getSFVec3f();
    infosEntrees[2]=0;
    infosEntrees[3]=0;
    infosEntrees[0]=vecTemp[0];
    infosEntrees[1]=vecTemp[2];
    vecTemp=coordKhep2->getSFVec3f();
    infosEntrees[6]=0;
    infosEntrees[7]=0;
    infosEntrees[4]=vecTemp[0];
    infosEntrees[5]=vecTemp[2];
    vecTemp=coordBall->getSFVec3f();
    infosEntrees[10]=0;
    infosEntrees[11]=0;
    infosEntrees[8]=vecTemp[0];
    infosEntrees[9]=vecTemp[2];
#endif
while(!arret && nbIterations < NB_ITERATION)
 {
     


    monCerveau->CalculerCoucheSortie(infosEntrees);
    coucheSortie = monCerveau->GetNeuronesSortie();
    //envoyer les infos au robot pour qu'il bouge
    #ifdef SIM
webots::Emitter* emit=pMaster->getEmitter("rs232_out");
emit->setRange(-1);
emit->setChannel(1);
emit->send((const void*)coucheSortie,2*sizeof(double));
emit->setChannel(2);
emit->send((const void*)(coucheSortie+2),2*sizeof(double));
cout<<"Data send";
#else
    #endif

    //si le jeu est fini (but marqué, balle arrêté, robot du mauvais côté de la ligne)
    #ifdef SIM
	vecTemp=coordKhep1->getSFVec3f();
	if(vecTemp[2]>coordBordsTerrain[2][1]/2)
	 {
		depFlag=true;
} 
   vecTemp=coordKhep2->getSFVec3f();
	if(vecTemp[2]>coordBordsTerrain[2][1]/2)
{
		depFlag=true;
}
double longTemp;

double longMin=sqrt(pow(coordBordsTerrain[0][0]-coordBordsTerrain[1][0],2)+pow(coordBordsTerrain[0][1]-coordBordsTerrain[1][1],2));
int mmin;
vecTemp=coordBall->getSFVec3f();
	for(int m=0;m<4;m++)
{
	longTemp=((coordBordsTerrain[m][1]-vecTemp[2])*(coordBordsTerrain[m][1]-coordBordsTerrain[(m+1)%4][1])-(coordBordsTerrain[m][0]-vecTemp[0])*(coordBordsTerrain[(m+1)%4][0]-coordBordsTerrain[(m)%4][0]))/sqrt(pow(coordBordsTerrain[(m+1)%4][0]-coordBordsTerrain[m][0],2)+pow(coordBordsTerrain[(m+1)%4][1]-coordBordsTerrain[m][1],2)); // fORMULE CALCULANT LA PROJECTION D'UN POINT SUR UNE DROITE
	if(longTemp<longMin)
	{
	longMin=longTemp;
	mmin=m;
	}
}
if (longMin<=radiusBall)
    if(mmin=0)
{
        butFlag=-1;
}
    else if(mmin=2)
{
       butFlag=1;}
 
    if(depFlag==true||butFlag!=0) 
	 arret=true;
        #else
    #endif
    nbIterations++;
const double vecTemp2[3]={vecTemp[0],vecTemp[1],-0.3};
coordBall->setSFVec3f(vecTemp2);

 // Récupération des infos en entrées : position balle et position robot dans un tableau de doubles infosEntrees
 #ifdef SIM
pMaster->step(TIME_STEP);
usleep(1000);

    //Pour chaque objet sur le terrain simulés,recupere lapositiondel'ojetenquestion,puiscalcullavitesseparraport a la position précedentes puis garde la nouvelle position.
    vecTemp=coordKhep1->getSFVec3f();
    infosEntrees[2]=vecTemp[0]-infosEntrees[0]/TIME_STEP;
    infosEntrees[3]=vecTemp[2]-infosEntrees[1]/TIME_STEP;
    infosEntrees[0]=vecTemp[0];
    infosEntrees[1]=vecTemp[2];
    vecTemp=coordKhep2->getSFVec3f();
    infosEntrees[6]=vecTemp[0]-infosEntrees[4]/TIME_STEP;
    infosEntrees[7]=vecTemp[2]-infosEntrees[5]/TIME_STEP;
    infosEntrees[4]=vecTemp[0];
    infosEntrees[5]=vecTemp[2];
    vecTemp=coordBall->getSFVec3f();
    infosEntrees[10]=vecTemp[0]-infosEntrees[8]/TIME_STEP;
    infosEntrees[11]=vecTemp[2]-infosEntrees[9]/TIME_STEP;
    infosEntrees[8]=vecTemp[0];
    infosEntrees[9]=vecTemp[2];

    #else

    #endif
 }
cout<<"Fin du jeu";
 //Récupération des données finales : position de la balle, si but marqué, position du robot
       #ifdef SIM
       infosSortie.xBalle=vecTemp[0];
       infosSortie.yBalle=vecTemp[2];
       infosSortie.butMarque=butFlag;
       infosSortie.depassement=depFlag;
      #endif
      //CALCUL  : si but marqué, si robot a dépassé la limite...


      return infosSortie;
}


//------------------------------------------------- Surcharge d'opérateurs


//-------------------------------------------- Constructeurs - destructeur
Robot::Robot()
{
  monCerveau = new Reseau();
  fitness = 0;
}

Robot::Robot(char * name)
{
  ifstream infile;
  infile.open (name, ifstream::in);
  infile.read ( (char*) &fitness, sizeof (fitness ) );
  monCerveau->Load(infile); 

}

Robot::~Robot()
{
}

//------------------------------------------------------------------ PRIVE

//----------------------------------------------------- Méthodes protégées
